%% 四架无人机
clc; clear; 
close all;
format long;

%% parameters
global dt Quadrotor_Num
global Tf
parameters;
t = 0 : dt : Tf;
N = Tf/dt + 1;
% UAV's flag
flag = zeros(Quadrotor_Num,1); 
for Num = 1:Quadrotor_Num
    flag(Num) = Num;
end
%% Initialization Position1
x0 = zeros(12,Quadrotor_Num + 1);
% 无人机状态
x0(:,1) = [0;   0;  0; 0; 0; 0;
           0.5; 0;  0.4; 0; 0; 0];   % Follower1
x0(:,2) = [0;   0;  0; 0; 0; 0;
           0.5; 0; -0.4; 0; 0; 0];   % Follower2
x0(:,3) = [0;   0;  0; 0; 0; 0;
          -0.5; 0;  0.4; 0; 0; 0];   % Follower3
x0(:,4) = [0;   0;  0; 0; 0; 0;
          -0.5; 0; -0.4; 0; 0; 0];   % Follower4
% 物体状态
x0(:,5) = [0; 0; 0; 0; 0; 0;
           0; 0; 0; 0; 0; 0]; % Object 几何位置
x = x0;
x_New = zeros(13,Quadrotor_Num + 1);
% 标记UAV
x_New(1,:) = 1;
x_New(2:13,:) = x;

% rho 物体质心指向悬挂边缘的点
rho = [0.5  0.5 -0.5 -0.5;
       0.4 -0.4  0.4 -0.4;
       0.1  0.1  0.1  0.1];

% q_i 无人机质心指向连接平台位置初始值(初始值为[0,0,1]'，这里写q_i为0，是用来计算q_i_dot)
qi = zeros(3,4);

% 无人机质心指向连接平台位置预期位置解算值初始值
qi_d = zeros(3,4);

% 绳索角速度初始值
sOmega = zeros(3,4);

% 保存输出画图数据
X_UAV_save = zeros(N,12, Quadrotor_Num);    % States
X_L_save = zeros(N,12);

e_3 = [0; 0; 1];

pL_d = zeros(3,N);
pL_d_dot = zeros(3, N);
RL_d = zeros(3,3, N);
Att_L_d = zeros(3,N);
Ome_L_d = zeros(3,N);

k1_plant = zeros(12,Quadrotor_Num); 
k2_plant = zeros(12,Quadrotor_Num);
k3_plant = zeros(12,Quadrotor_Num); 
k4_plant = zeros(12,Quadrotor_Num);

% 记录无人机姿态角速度的预期值，要经过迭代
omegaid_p = zeros(3,4);
sOmega_d = zeros(3,4);
Att_L_d_Last = zeros(3,1);

Ome_L = zeros(3,1);

% (sOmega_d(k + 1) - sOmega_d(k)) / dt
for i = 1:N
    % 货物运输轨迹Desire of Position
    omega_d = 2 * pi / 40;
    xl_d = 1.5 * sin(omega_d * t(i));
    yl_d = 4.2 * cos(omega_d * t(i) / 2);
    zl_d = 0.5;
    pL_d(:,i) = [xl_d yl_d zl_d]';
    
    xl_d_dot = 1.5 * omega_d * cos(omega_d * t(i));
    yl_d_dot = -4.2 * 0.5 * omega_d * sin(omega_d * t(i) / 2);
    zl_d_dot = 0;
    pL_d_dot(:,i) = [xl_d_dot yl_d_dot zl_d_dot]';

    % 预期姿态
    Rl_d1 = pL_d_dot(:,i) / norm(pL_d_dot(:,i));
    Rl_d2 = skew(e_3) * pL_d_dot(:,i) / norm(skew(e_3) * pL_d_dot(:,i));
    RL_d(:,:, i) = [Rl_d1 Rl_d2 e_3];

    angles_eul = rotm2eul(RL_d(:,:, i),'ZYX');
    Att_L_d(:,i) = [angles_eul(3); angles_eul(2); angles_eul(1)];
    % Att_L_d(:,i) = rotation_matrix_to_euler(RL_d(:,:, i));
    Ome_L_d(:,i) = (Att_L_d(:,i) - Att_L_d_Last)/ dt;
    Att_L_d_Last = Att_L_d(:,i);

    [F_in, M_in, F_plant, M_plant, qi_d, qi, a_all, ui_n, sOmega, sOmega_d, omegaid_p, Ome_L] = controller_o_i(x_New, qi_d, sOmega, sOmega_d, qi, Att_L_d(:,i), Ome_L, Ome_L_d(:,i), pL_d(:,i), pL_d_dot(:,i), omegaid_p, rho);

    % 传入到货物中
    k1_o = plant_load(x_New(2:13,5),             F_in, M_in);
    k2_o = plant_load(x_New(2:13,5) + dt/2*k1_o, F_in, M_in);
    k3_o = plant_load(x_New(2:13,5) + dt/2*k2_o, F_in, M_in);
    k4_o = plant_load(x_New(2:13,5) + dt * k3_o, F_in, M_in);
    x_New(2:13,5) = x_New(2:13,5) + dt/6 *(k1_o + 2*k2_o + 2*k3_o + k4_o);

    % 传入到无人机和绳索中
    for plant_i = 1 : Quadrotor_Num
        % 无人机
        k1_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i),                               F_plant(plant_i), M_plant(:,plant_i));
        k2_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i) + dt/2 * k1_plant(:, plant_i), F_plant(plant_i), M_plant(:,plant_i));
        k3_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i) + dt/2 * k2_plant(:, plant_i), F_plant(plant_i), M_plant(:,plant_i));
        k4_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i) + dt   * k3_plant(:, plant_i), F_plant(plant_i), M_plant(:,plant_i));
        x_New(2:13,plant_i) = x_New(2:13,plant_i) + dt/6 * (k1_plant(:, plant_i) + 2 * k2_plant(:, plant_i) + 2 * k3_plant(:, plant_i) + k4_plant(:, plant_i));
    end
    
    for plant_i = 1 : 4
        X_UAV_save(i,:,plant_i) = x_New(2:13,plant_i);
    end
    X_L_save(i,:) = x_New(2:13,5);


end

%% 3-D Space
% steps
stepping = 500; 
% Linestyles
marker_shapes = cell(1,Quadrotor_Num);
for Num = 1:Quadrotor_Num
    shapes = {'^', 'd', 's', 'o', '+', '*', 'x'};
    marker_shapes{Num} = shapes{mod(Num - 1, length(shapes)) + 1};
end

figure
for Num = 1:Quadrotor_Num
    plot3(X_UAV_save(1:stepping:N, 7, Num), X_UAV_save(1:stepping:N, 9, Num), X_UAV_save(1:stepping:N, 11, Num), marker_shapes{Num}, 'Linewidth', 2);
    hold on;
end
for Num = 1:Quadrotor_Num
    plot3(X_UAV_save(1:stepping:N, 7, Num), X_UAV_save(1:stepping:N, 9, Num), X_UAV_save(1:stepping:N, 11, Num),'Linewidth', 2);
    hold on;
end
pointNUM = 1:stepping:N;
for Num = pointNUM
     %% Four UAVS
     for j = 1:Quadrotor_Num
        plot3([X_UAV_save(Num,7,j),  X_UAV_save(Num,7,mod(j,Quadrotor_Num) + 1)], ...
              [X_UAV_save(Num,9,j),  X_UAV_save(Num,9,mod(j,Quadrotor_Num) + 1)], ...
              [X_UAV_save(Num,11,j), X_UAV_save(Num,11,mod(j,Quadrotor_Num) + 1)], 'Linewidth', 1.5);
        hold on
     end
end

figure 
plot3(X_L_save(:,7),X_L_save(:,9),X_L_save(:,11),'Color','#0000FF','Linewidth',3);
title('line in 3-D Space');
xlabel('X');
ylabel('Y');
zlabel('Z');hold on
grid on;

%% 预期的轨迹
figure 
plot3(pL_d(1,:),pL_d(2,:),pL_d(3,:),'Color','#0000FF','Linewidth',3);
title('line in 3-D Space');
xlabel('X');
ylabel('Y');
zlabel('Z');hold on
grid on;

figure
% Position 
quatrotor_plot(t, pL_d(1,:),'-b'); 
hold on
quatrotor_plot(t, pL_d(2,:),'-r');
quatrotor_plot(t, pL_d(3,:), '-g')
grid on;
title('Position Response');
legend('X','Y','Z');

%% 绘制期望偏航角psi角
figure
% Position 
quatrotor_plot(t, Att_L_d(3,:),'-b'); 
hold on;
quatrotor_plot(t, Ome_L_d(3,:),'-b'); 
title('期望偏航角psi/偏航角速度');
legend('/psi','/psi_dot');

figure
% Position 
quatrotor_plot(t, X_L_save(:,7),'-b'); 
hold on
quatrotor_plot(t, X_L_save(:,9),'-r');
quatrotor_plot(t, X_L_save(:,11), '-g')
grid on;
title('Position Response');
legend('X','Y','Z');


figure
% Position 
quatrotor_plot(t, X_UAV_save(:,7,1),'-b'); 
hold on
quatrotor_plot(t, X_UAV_save(:,9,1),'-r');
quatrotor_plot(t, X_UAV_save(:,11,1), '-g')
grid on;
title('Position Response');
legend('X','Y','Z');


%% 
function v = vee(S)
    % S 为 3x3 斜对称矩阵, 返回对应的 3x1 向量
    v = [S(3,2); S(1,3); S(2,1)];
end
%% ================== 斜对称矩阵生成函数 ==================
function S = skew(v)
    % v 为 3x1 向量, S 则为 3x3 反对称矩阵,满足 S*w = v x w
    S = [   0,   -v(3),  v(2);
         v(3),     0,   -v(1);
        -v(2),   v(1),    0   ];
end

function euler_angles = rotation_matrix_to_euler(R)
    % 确保矩阵是3x3
    if size(R,1) ~= 3 || size(R,2) ~= 3
        error('输入的旋转矩阵必须是3x3矩阵');
    end
    
    % 提取旋转矩阵的元素
    R11 = R(1,1);
    R12 = R(1,2);
    R13 = R(1,3);
    R21 = R(2,1);
    R22 = R(2,2);
    R23 = R(2,3);
    R31 = R(3,1);
    R32 = R(3,2);
    R33 = R(3,3);
    
    % 计算欧拉角（ZYX顺序）
    yaw = atan2(R21, R11);  % 绕Z轴的旋转
    pitch = atan2(-R31, sqrt(R11^2 + R21^2));  % 绕Y轴的旋转
    roll = atan2(R32, R33);  % 绕X轴的旋转
    
    % 返回欧拉角
    euler_angles = [roll, pitch, yaw]';  % 顺序为 [roll, pitch, yaw]
end